#include <buola/scene.h>
#include <buola/scene/csceneview.h>
#include <buola/scene/ccamera.h>
#include <buola/scene/cperspectivecamera.h>
#include <buola/scene/cscene.h>
#include <buola/scene/geodes/ccube.h>
#include <buola/scene/geodes/ccylinder.h>
#include <buola/scene/geodes/ccapsule.h>
#include <buola/scene/geodes/cpointcloud.h>
#include <buola/scene/transform/crttransform.h>
#include <buola/scene/transform/crevolutionjoint.h>
#include <buola/scene/transform/cprismaticjoint.h>
#include <buola/scene/cimagerenderer.h>
#include <buola/scene/cmesh.h>
#include <buola/scene/controllers/ccameracontroller.h>
#include <buola/image/io.h>
#include <buola/image/cgc.h>
#include <buola/image/algorithm/calibration.h>
#include <buola/gui/ctoolkit.h>
#include <buola/app/capp.h>
#include <buola/io/cresourcefinder.h>

using namespace buola;

void set_joint(scene::PTransform pWhere,const std::string &pName,double pValue)
{
    auto lRev=scene::find_revolution_joint(pWhere,pName);
    if(lRev)
    {
        lRev->SetValue(pValue);
        return;
    }
    auto lPri=scene::find_prismatic_joint(pWhere,pName);
    if(lPri)
    {
        msg_info() << "setting prismatic joint " << lPri->Name() << " to " << pValue << "\n";
        lPri->SetValue(pValue);
        return;
    }
    
    msg_warn() << "can't find joint!!!!\n";
}

int main(int pNArg,char **pArgs)
{
    buola_init(pNArg,pArgs);

    try
    {
        CApp lApp;

        gui::get_toolkit()->GetOpenGLGC()->MakeCurrent();
        
        //create the scene
        scene::PScene lScene=new scene::CScene;
        
        scene::PTransform lObject=scene::CTransform::Import(io::arg(0));
        lScene->World()->AddChild(lObject);

        set_joint(lObject,"fl_caster_rotation_joint",-0.00533353);
        set_joint(lObject,"fl_caster_l_wheel_joint",-0.682664);
        set_joint(lObject,"fl_caster_r_wheel_joint",1.47152);
        set_joint(lObject,"fr_caster_rotation_joint",-0.00249213);
        set_joint(lObject,"fr_caster_l_wheel_joint",-1.02106);
        set_joint(lObject,"fr_caster_r_wheel_joint",1.03738);
        set_joint(lObject,"bl_caster_rotation_joint",3.13362);
        set_joint(lObject,"bl_caster_l_wheel_joint",-1.43392);
        set_joint(lObject,"bl_caster_r_wheel_joint",0.604624);
        set_joint(lObject,"br_caster_rotation_joint",3.13606);
        set_joint(lObject,"br_caster_l_wheel_joint",-1.35575);
        set_joint(lObject,"br_caster_r_wheel_joint",0.711012);
        set_joint(lObject,"torso_lift_joint",0.0115185);
        set_joint(lObject,"torso_lift_motor_screw_joint",0);
        set_joint(lObject,"head_pan_joint",0.138643);
        set_joint(lObject,"head_tilt_joint",0.944153);
        set_joint(lObject,"laser_tilt_mount_joint",0.276684);
        set_joint(lObject,"r_upper_arm_roll_joint",0.471974);
        set_joint(lObject,"r_shoulder_pan_joint",0.290133);
        set_joint(lObject,"r_shoulder_lift_joint",-0.220643);
        set_joint(lObject,"r_forearm_roll_joint",-39.8391);
        set_joint(lObject,"r_elbow_flex_joint",-0.445143);
        set_joint(lObject,"r_wrist_flex_joint",-1.9016);
        set_joint(lObject,"r_wrist_roll_joint",22.6797);
        set_joint(lObject,"r_gripper_joint",0.0634837);
        set_joint(lObject,"r_gripper_l_finger_joint",0.361588);
        set_joint(lObject,"r_gripper_r_finger_joint",0.361588);
        set_joint(lObject,"r_gripper_r_finger_tip_joint",0.361588);
        set_joint(lObject,"r_gripper_l_finger_tip_joint",0.361588);
        set_joint(lObject,"r_gripper_motor_screw_joint",0);
        set_joint(lObject,"r_gripper_motor_slider_joint",0);
        set_joint(lObject,"l_upper_arm_roll_joint",0.47169);
        set_joint(lObject,"l_shoulder_pan_joint",0.91381);
        set_joint(lObject,"l_shoulder_lift_joint",-0.229666);
        set_joint(lObject,"l_forearm_roll_joint",40.265);
        set_joint(lObject,"l_elbow_flex_joint",-0.268668);
        set_joint(lObject,"l_wrist_flex_joint",-2.00228);
        set_joint(lObject,"l_wrist_roll_joint",4.82731);
        set_joint(lObject,"l_gripper_joint",0.0188612);
        set_joint(lObject,"l_gripper_l_finger_joint",0.105917);
        set_joint(lObject,"l_gripper_r_finger_joint",0.105917);
        set_joint(lObject,"l_gripper_r_finger_tip_joint",0.105917);
        set_joint(lObject,"l_gripper_l_finger_tip_joint",0.105917);
        set_joint(lObject,"l_gripper_motor_screw_joint",0);
        set_joint(lObject,"l_gripper_motor_slider_joint",0);
        
        lObject->PrintTransformTree();
        lScene->Update();
        scene::PTransform lOpticalFrame=lObject->FindDescendant("head_mount_kinect_rgb_optical_frame");
        msg_info() << lOpticalFrame->Name() << "\n";
        mat::CMat34d lTrans=lOpticalFrame->GetWorldTransform();
        msg_info() << lTrans << "\n";
        msg_info() << lTrans*mat::CVec3d(0,0,0) << "\n";
        msg_info() << mat::CQuat_d(lTrans) << "\n";

        if(cmd_line().ArgCount()>=2)
        {
            scene::PPointCloud lCloud=new scene::CPointCloud();
            lOpticalFrame->AddObject(lCloud);
            lCloud->SetCloud(cvi::load_cloud_pcd(io::arg(1)));
        }
        
        scene::PPerspectiveCamera lCamera=new scene::CPerspectiveCamera;
//        lCamera->LookAt({0,0,1},{5,0,1},{0,0,1});
        img::CCameraParameters lParams;
        lParams.SetMatrix(CPoint_d(525,525),CPoint_d(320,240));
        lCamera->SetShift(lParams.GetC()-CSize_d(320,240),CSize_d(640,480));
        lCamera->SetFOV(2*atan2(240,lParams.GetF().y));
        lCamera->LookFrom(lTrans*mat::CVec3d(0,0,0),mat::CQuat_d(lTrans)*mat::CQuat_d(90_deg,mat::CVec3d(1,0,0)));
        lCamera->SetClipping(0.1,50.0);

        scene::CImageRenderer lRenderer;
        lRenderer.SetCamera(lCamera);
        lRenderer.AddScene(lScene);

        scene::PSceneView lView=new scene::CSceneView;
        lView->SetCamera(lCamera);
        lView->AddScene(lScene);
        lView->AddController(new scene::CCameraController(lCamera));
        lView->Map(CSize_d(800,600));
        
        lApp.Run();
    }
    catch(std::exception &pE)
    {
        msg_info() << pE.what() << "\n";
    }

    return buola_finish();
}
